<?php
/**
 * Created by PhpStorm.
 * User: junyu
 * Date: 2019/7/21
 * Time: 下午5:23
 */


class Gpsfix{
    public $lastDatas=[]; //最近的GPS点
    public $lastDrcts=[]; //最近的方向点
    public $magps; //平滑后的GPS点
    public $drct; //计算后的方向
    public $madrct; //平滑后的方向
    public $gpsPass; //是否为有效点
    public $started=0; //是否开始fix修正

    #计算两点之间的距离 返回米
    function getDistance( $_lat1, $_lon1,  $_lat2, $_lon2){
        $d = 0;
        try {
            $lat1 = (pi() / 180) * $_lat1;
            $lat2 = (pi() / 180) * $_lat2;

            $lon1 = (pi() / 180) * $_lon1;
            $lon2 = (pi() / 180) * $_lon2;
            #地球半径
            $R = 6378.1;

            $d = acos(sin($lat1) * sin($lat2) + cos($lat1) * cos($lat2) * cos($lon2 - $lon1)) * $R * 1000;
        }catch (Exception $e) {

        }

        return $d;
    }


    function gps2d($lat_a,$lng_a,$lat_b,$lng_b){
        $d=0;
        
        $lat_a=$lat_a*pi()/180;
        $lat_b=$lat_b*pi()/180;
        $lng_a=$lng_a*pi()/180;
        $lng_b=$lng_b*pi()/180;

        $lng_def = $lng_b-$lng_a;
        $lat_def = $lat_b-$lat_a;


        $d= sin($lat_a)*sin($lat_b) + cos($lat_a)*cos($lat_b)*cos($lng_b-$lng_a);
        $d=sqrt(1-$d*$d);
        if(!is_nan($d)){
            $d=cos($lat_b)*sin($lng_b-$lng_a)/$d;
            $d=asin($d)*180/pi();
            if($lat_def<0){
                if($lng_def>0){
                   $d=180-$d ; 
               }else{
                   $d=abs($d)+180 ;
               }
                
            }else if($lat_def==0){
                if($lat_def>0){
                    $d=90;
                }
                if($lat_def<0){
                    $d=180;
                }

            }else if($lat_def<0){
                 $d=$d+360;
            }

            if($d<0) $d = 360+$d;
	    if(is_nan($d)){
               $d=$this->drct;
            }
             $this->drct = $d;
        }else{
            $d = $this->drct;
        }

        $this->drct = $d;

        $this->drct= $this->getmaDrct($this->drct, 30);
        return $this->drct;
    }

    function getmaDrct($drct,$lastavgNum=15){
        $this->lastDrcts[]=$drct;

        if(count($this->lastDrcts)>$lastavgNum){
            array_shift($this->lastDrcts);
        }
        $this->madrct = array_sum($this->lastDrcts) / count($this->lastDrcts);
        return $this->madrct;
    }

    # 返回最近的N个点
    function getnearPoint($lat,$lon,$drct,$time,$nums=3){
        $this->lastDatas[]=[$lat,$lon,$drct,$time];
        if (count($this->lastDatas)>$nums){
            array_shift($this->lastDatas);
        }

        return $this->lastDatas;
    }

    #返回120个点

    function getDevPoint($datas,$startNum=300,$avg=200){
        $data=[];
        if (count($datas)<$startNum){
            // 如果点不够则读取数据库最近15分钟内的记录
            
            return false;
        }
        $this->started=1;

        $before = array_slice($datas,0,$avg);
        $after = array_slice($datas,count($datas)-$avg);

        $data['before'] = [
            array_sum(array_column($before,0))/$avg ,
            array_sum(array_column($before,1))/$avg,
            $before[$avg-1][2],$before[$avg-1][3]];
        $data['after'] = [
            array_sum(array_column($after,0))/$avg ,
            array_sum(array_column($after,1))/$avg,
            $after[$avg-1][2],$after[$avg-1][3]];
        $this->magps['lat'] = $data['after'][0];
        $this->magps['lon'] = $data['after'][1];
        echo "fix pass\r\n";
//        $this->magps['lat'] = round($data['after'][] / $avg,6);
        return $data;

    }

    function insertData($tableName,$data){

        $s="insert into $tableName (";
        foreach($data as $k=>$v){
            $s.= '`"'.$k."`,";
        }
        $s=  substr($s,0,-1) .") values(";
        foreach($data as $k=>$v){
            $s.= "'".$v."'";
        }
        echo $s;
        return $s;
    }

    function fix($row,$minGpsNum=300,$magps=0,$db=false){
     //   获取最近的300个点
        $this->gpsPass=1;
        if($row['drct']==0){
           $row['drct']=$this->drct;
        }

        $lastPoint = $this->getnearPoint($row['lat'] , $row['lon'], $row['drct'] ,$row['gps_time'],$minGpsNum);
//    echo json_encode($lastPoint);
        echo "\r\n\r\n";
        //获取开始的点,及结束的点
        $points = $this->getDevPoint($lastPoint,$minGpsNum,round($minGpsNum/4));
        if(!$row['drct']) $row['drct'] = $this->drct;
        if($magps && isset($this->magps['lat'])){
            $row['lat'] = $this->magps['lat'];
            $row['lon'] = $this->magps['lon'];
        }

        if(!$points){
            // $row['drct'] = 100;
            echo "point ---- min:". $minGpsNum. " now:". count($lastPoint) ." \r\n";
            if($row['drct']==0){
            	//如果刚启动，未启动修正前，尝试加载60分钟内最后120个有效方向的均值
            	if($db){
            		$sql="select ROUND(avg(drct),1) drct from gpsinfo where sid=$row[sid] and drct>5 and gps_time>FROM_UNIXTIME(UNIX_TIMESTAMP()-3600,'%Y-%m-%d %H:%i:%s') order by gps_time limit 120";
					$drctRes = $db->select($sql);
					if($drctRes[0]['drct']){
                        echo "SQL DRCT:".$drctRes[0]['drct']."\r\n";
					    $this->drct =$drctRes[0]['drct'];
					}
            	}
                
                
                $row['drct'] = $this->drct;
            }else{
	           $this->drct = $row['drct'];
           }
           echo "\r\nNComputeDrct:".$this->drct." \r\n";
           return $row;
        }
        //计算距离
        $dist=$this->getDistance($points['before'][0],$points['before'][1],$points['after'][0],$points['after'][1]);
        if($row['dmod']>2 and $dist<0.4){
                $this->gpsPass=0;
    	        $row['drct'] = $this->drct;
                return $row;
    	}
	

        if($row['dmod']<3 && $dist<1.6){
            $this->gpsPass=0;
            $row['drct'] = $this->drct;
            echo "\r\ndist low 1.2";
            return $row;
        }

        $this->gps2d($points['before'][0],$points['before'][1],$points['after'][0],$points['after'][1]);

        $row['drct'] = $this->drct;
        return $row;
    }

}

$gpsfix = new Gpsfix();
echo $gpsfix->gps2d(31.933686089286,109.52923463214,31.933719026191,109.52921679524);

// $conn = mysqli_connect('127.0.0.1','root','','gpsdata');
// $datares = mysqli_query($conn,"select * from jz_pave_gpsdata_310_34 limit 8000");
// while($row =mysqli_fetch_assoc($datares)){
//     //
//     $row = $gpsfix->fix($row,200,1);
//     if(!$gpsfix->gpsPass) continue;
//     unset($row['id']);
//     $row['sid']=59;
//     $row['paver_layer_id']=34;
//     echo $row['gps_time']."\r\n";
//     $sql = "insert into gpsinfo (sid,lat,lon,drct,paver_layer_id,roller_layer_id,hi,speed,gps_time)
//                 values ($row[sid],$row[lat],$row[lon],$row[drct],$row[paver_layer_id],$row[roller_layer_id],$row[hi],$row[speed],'$row[gps_time]')";
//     $conn->query($sql);
//     $sql2 = "update carlist set lat=$row[lat],lon=$row[lon],speed=$row[speed],drct=$row[drct] where sid=59";
//     $conn->query($sql2);

// //    echo json_encode($row);
//     usleep(100000);
// }
